#include "HectorMappingRos.h"

#include "map/GridMap.h"
#include "HectorMapMutex.h"

#include "generaltype.h"

#include "lidar_main.h"


// #ifndef TF_SCALAR_H
//   typedef btScalar tfScalar;
// #endif

HectorMappingRos::HectorMappingRos()
  : lastGetMapUpdateIndex(-100)
//   , map__publish_thread_(0)
  , initial_pose_set_(false)
  , pause_scan_processing_(false)
{
  std::string mapTopic_ = "map";

  p_pub_drawings=false;
  p_pub_debug_output_=false;
  p_pub_map_odom_transform_=true; //true
  p_pub_odometry_=false; //false
  p_advertise_map_service_=false; //true
  p_scan_subscriber_queue_size_=5; //5

  p_map_resolution_=0.1; //0.25
  p_map_size_=512; //1024
  p_map_start_x_=0.5; //0.5
  p_map_start_y_=0.5; //0.5
  p_map_multi_res_levels_=1; //3

  p_update_factor_free_=0.4; //0.4
  p_update_factor_occupied_=0.9; //0.9

  p_map_update_distance_threshold_=0.4; //0.4
  p_map_update_angle_threshold_=0.2; //0.9

  p_scan_topic_=std::string("scan"); //std::string("scan")
  p_sys_msg_topic_=std::string("syscommand"); //std::string("syscommand")
  p_pose_update_topic_=std::string("poseupdate"); //std::string("poseupdate")

  p_use_tf_scan_transformation_=false; //true
  p_use_tf_pose_start_estimate_=false; //false
  p_map_with_known_poses_=false; //false

  p_base_frame_=std::string("base_link"); //std::string("base_link")
  p_map_frame_=std::string("map"); //std::string("map")
  p_odom_frame_=std::string("base_link"); // std::string("odom")

  p_pub_map_scanmatch_transform_=true;
  p_tf_map_scanmatch_transform_frame_name_=std::string("scanmatcher_frame"); //std::string("scanmatcher_frame")

  p_timing_output_=false; //false

  p_map_pub_period_=2.0; //2.0

  double tmp = 0.0;
  tmp=0.4; //laser_min_dist=0.4
  p_sqr_laser_min_dist_ = static_cast<float>(tmp*tmp);

  tmp=30.0; //laser_max_dist=30.0
  p_sqr_laser_max_dist_ = static_cast<float>(tmp*tmp);

  tmp=-1.0; //laser_z_min_value=-1.0
  p_laser_z_min_value_ = static_cast<float>(tmp);

  tmp=1.0; //laser_z_max_value=1.0
  p_laser_z_max_value_ = static_cast<float>(tmp);

  slamProcessor = new hectorslam::HectorSlamProcessor(static_cast<float>(p_map_resolution_), p_map_size_, p_map_size_, Eigen::Vector2f(p_map_start_x_, p_map_start_y_), p_map_multi_res_levels_/*2*/);
  slamProcessor->setUpdateFactorFree(p_update_factor_free_);
  slamProcessor->setUpdateFactorOccupied(p_update_factor_occupied_);
  slamProcessor->setMapUpdateMinDistDiff(p_map_update_distance_threshold_);
  slamProcessor->setMapUpdateMinAngleDiff(p_map_update_angle_threshold_);

  int mapLevels = slamProcessor->getMapLevels();
  mapLevels = 1;

  for (int i = 0; i < mapLevels; ++i)
  {
    // mapPubContainer.push_back(MapPublisherContainer());
    slamProcessor->addMapMutex(i, new HectorMapMutex());

    std::string mapTopicStr(mapTopic_);


    std::string mapMetaTopicStr(mapTopicStr);
    mapMetaTopicStr.append("_metadata");

  }


  printf("HectorSM p_base_frame_: %s", p_base_frame_.c_str());
  printf("HectorSM p_map_frame_: %s", p_map_frame_.c_str());
  printf("HectorSM p_odom_frame_: %s", p_odom_frame_.c_str());
  printf("HectorSM p_scan_topic_: %s", p_scan_topic_.c_str());
  printf("HectorSM p_use_tf_scan_transformation_: %s", p_use_tf_scan_transformation_ ? ("true") : ("false"));
  printf("HectorSM p_pub_map_odom_transform_: %s", p_pub_map_odom_transform_ ? ("true") : ("false"));
  printf("HectorSM p_scan_subscriber_queue_size_: %d", p_scan_subscriber_queue_size_);
  printf("HectorSM p_map_pub_period_: %f", p_map_pub_period_);
  printf("HectorSM p_update_factor_free_: %f", p_update_factor_free_);
  printf("HectorSM p_update_factor_occupied_: %f", p_update_factor_occupied_);
  printf("HectorSM p_map_update_distance_threshold_: %f ", p_map_update_distance_threshold_);
  printf("HectorSM p_map_update_angle_threshold_: %f", p_map_update_angle_threshold_);
  printf("HectorSM p_laser_z_min_value_: %f", p_laser_z_min_value_);
  printf("HectorSM p_laser_z_max_value_: %f", p_laser_z_max_value_);
}

HectorMappingRos::~HectorMappingRos()
{
  delete slamProcessor;

}

void HectorMappingRos::scanCallback(const HiRobot::LaserScan& scan)
{
if (pause_scan_processing_)
  {
    return;
  }

  if (!p_use_tf_scan_transformation_)
  {
    // If we are not using the tf tree to find the transform between the base frame and laser frame,
    // then just convert the laser scan to our data container and process the update based on our last
    // pose estimate
    this->rosLaserScanToDataContainer(scan, laserScanContainer, slamProcessor->getScaleToMap());
    slamProcessor->update(laserScanContainer, slamProcessor->getLastScanMatchPose());
  }

  // If we're just building a map with known poses, we're finished now. Code below this point publishes the localization results.
  if (p_map_with_known_poses_)
  {
    return;
  }

  poseInfoContainer_.update(slamProcessor->getLastScanMatchPose(), slamProcessor->getLastScanMatchCovariance());

//   posePublisher_.publish(poseInfoContainer_.getPoseStamped()); //直接调用poseInfoContainer_.getPoseStamped()取得
}


void HectorMappingRos::rosLaserScanToDataContainer(const HiRobot::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap)
{
  size_t size = scan.ranges.size();

  float angle = scan.angle_min;

  dataContainer.clear();

  dataContainer.setOrigo(Eigen::Vector2f::Zero());

  float maxRangeForContainer = scan.range_max - 0.1f;

  for (size_t i = 0; i < size; ++i)
  {
    float dist = scan.ranges[i];

    if ( (dist > scan.range_min) && (dist < maxRangeForContainer))
    {
      dist *= scaleToMap;
      dataContainer.add(Eigen::Vector2f(cos(angle) * dist, sin(angle) * dist));
    }

    angle += scan.angle_increment;
  }
}


void HectorMappingRos::toggleMappingPause(bool pause)
{
  // Pause/unpause
  if (pause && !pause_scan_processing_)
  {
    printf("[HectorSM]: Mapping paused");
  }
  else if (!pause && pause_scan_processing_)
  {
    printf("[HectorSM]: Mapping no longer paused");
  }
  pause_scan_processing_ = pause;
}

void HectorMappingRos::resetPose(const HiRobot::RobotPose &pose)
{
  Eigen::Quaterniond tmp(pose.orientation.w,pose.orientation.x,pose.orientation.y,pose.orientation.z);
  initial_pose_set_ = true;
  initial_pose_ = Eigen::Vector3f(pose.position.x, pose.position.y, util::getYawFromQuat(tmp));
  printf("[HectorSM]: Setting initial pose with world coords x: %f y: %f yaw: %f",
           initial_pose_[0], initial_pose_[1], initial_pose_[2]);
}
